#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
RK3588客户端 - 整合所有功能
"""
import subprocess
import socket
import json
import time
import threading
import re
import struct
import math
import os
import glob
from datetime import datetime

try:
    import serial
except ImportError:
    serial = None

try:
    from pymodbus.client import ModbusSerialClient
except ImportError:
    try:
        from pymodbus.client.sync import ModbusSerialClient
    except ImportError:
        ModbusSerialClient = None

class PLCController:
    """PLC控制器 - 支持自动识别USB转RS485端口"""
    def __init__(self, port=None):
        self.port = port
        self.client = None
        self.connected = False
        
    def detect_plc_port(self):
        """自动检测PLC的USB转RS485端口"""
        if not ModbusSerialClient:
            print("[PLC检测] pymodbus未安装，无法检测")
            return None
        
        # 获取所有可能的USB串口设备
        possible_ports = []
        
        # 方法1：使用glob扫描USB串口
        usb_ports = glob.glob('/dev/ttyUSB*')
        possible_ports.extend(usb_ports)
        
        # 方法2：检查常见的串口设备
        common_ports = ['/dev/ttyACM0', '/dev/ttyACM1', '/dev/ttyAMA0']
        for port in common_ports:
            if os.path.exists(port):
                possible_ports.append(port)
        
        # 去重并排序
        possible_ports = sorted(list(set(possible_ports)))
        
        if not possible_ports:
            print("╔════════════════════════════════════════════════════════╗")
            print("║  ⚠️  未找到USB串口设备                                 ║")
            print("║                                                        ║")
            print("║  请检查：                                              ║")
            print("║  1. USB转RS485设备是否已插入                           ║")
            print("║  2. 设备驱动是否正确加载                              ║")
            print("║  3. 运行 'ls -l /dev/tty*' 查看所有串口               ║")
            print("╚════════════════════════════════════════════════════════╝")
            return None
        
        print(f"[PLC检测] 发现 {len(possible_ports)} 个串口设备: {possible_ports}")
        
        # 尝试连接每个端口
        for port in possible_ports:
            print(f"[PLC检测] 测试端口: {port}")
            
            try:
                # 尝试打开串口
                test_client = ModbusSerialClient(
                    port=port,
                    baudrate=9600,
                    bytesize=8,
                    parity='N',
                    stopbits=1,
                    timeout=2  # 2秒超时，确保PLC有足够响应时间
                )
                
                if test_client.connect():
                    print(f"[PLC检测]   ✓ 串口打开成功")
                    
                    # 给PLC一点时间准备
                    time.sleep(0.5)
                    
                    # 尝试读取寄存器120（控制权限）来验证是否是PLC
                    try:
                        print(f"[PLC检测]   正在读取寄存器D120...")
                        result = test_client.read_holding_registers(
                            address=120, 
                            count=1, 
                            slave=1
                        )
                        
                        if hasattr(result, 'registers'):
                            value = result.registers[0]
                            print(f"[PLC检测]   ✓ Modbus响应成功! 寄存器D120值: {value}")
                            print(f"[PLC检测]   ★ 识别到PLC设备: {port}")
                            test_client.close()
                            return port
                        else:
                            print(f"[PLC检测]   ✗ Modbus无响应: {result}")
                    except Exception as e:
                        print(f"[PLC检测]   ✗ Modbus通信失败: {e}")
                        print(f"[PLC检测]   提示: 请检查PLC是否通电，485线是否正确连接")
                    
                    test_client.close()
                else:
                    print(f"[PLC检测]   ✗ 串口打开失败")
                    
            except Exception as e:
                print(f"[PLC检测]   ✗ 测试失败: {e}")
            
            # 短暂延迟，避免设备冲突
            time.sleep(0.2)
        
        print("[PLC检测] ⚠️  未找到PLC设备")
        return None
        
    def connect(self):
        """连接PLC（自动检测端口）"""
        if not ModbusSerialClient:
            print("[PLC] pymodbus未安装")
            return False
        
        # 如果没有指定端口，自动检测
        if not self.port:
            print("[PLC] 开始自动检测USB转RS485端口...")
            self.port = self.detect_plc_port()
            
            if not self.port:
                print("[PLC] ⚠️  自动检测失败，PLC功能不可用")
                return False
        
        # 连接到检测到的端口
        try:
            print(f"[PLC] 连接到端口: {self.port}")
            self.client = ModbusSerialClient(
                port=self.port, 
                baudrate=9600, 
                bytesize=8,
                parity='N', 
                stopbits=1, 
                timeout=2
            )
            
            if self.client.connect():
                self.connected = True
                print(f"[PLC] ✓ 连接成功: {self.port}")
                
                # 验证连接
                try:
                    result = self.client.read_holding_registers(address=120, count=1, slave=1)
                    if hasattr(result, 'registers'):
                        print(f"[PLC] ✓ 验证成功，控制权限状态: {result.registers[0]}")
                    return True
                except:
                    print(f"[PLC] ⚠️  连接成功但验证失败")
                    return True
            else:
                print(f"[PLC] ✗ 连接失败: {self.port}")
        except Exception as e:
            print(f"[PLC] ✗ 连接异常: {e}")
        
        return False
    
    def read(self, addr):
        if not self.connected:
            return None
        try:
            result = self.client.read_holding_registers(address=addr, count=1, slave=1)
            if hasattr(result, 'registers'):
                v = result.registers[0]
                return v - 65536 if v > 32767 else v
        except:
            pass
        return None
    
    def write(self, addr, value):
        if not self.connected:
            return False
        try:
            if value < 0:
                value += 65536
            self.client.write_register(address=addr, value=value, slave=1)
            return True
        except:
            return False
    
    def read_all(self):
        """读取所有传感器"""
        return {
            'generator_voltage': self.read(129),
            'generator_current': self.read(130),
            'generator_power': self.read(131),
            'generator_temp': self.read(132),
            'engine_temp': self.read(133),
            'hot_water_temp': self.read(134),
            'cooling_temp': self.read(135),
            'box_temp': self.read(136),
            'fuel_level': self.read(137),
            'water_level': self.read(138),
            'control_authority': self.read(120),
            'relay_220v': self.read(124),
            'water_spray': self.read(125),
            'speed_percent': self.read(127)
        }

class MotionController:
    """运动控制器 - 平滑控制和安全限制"""
    def __init__(self, plc_controller):
        self.plc = plc_controller
        self.current_forward = 0
        self.current_turn = 0
        self.target_forward = 0
        self.target_turn = 0
        self.max_forward_speed = 30  # 最大前进速度
        self.max_turn_speed = 15     # 最大转向速度
        self.acceleration = 2         # 加速度（单位/更新周期）
        self.emergency_stop = False
        self.control_thread = None
        self.running = False
        
    def start(self):
        """启动运动控制循环"""
        self.running = True
        self.control_thread = threading.Thread(target=self._control_loop, daemon=True)
        self.control_thread.start()
        if self.plc.connected:
            print("[运动控制] ✓ 已启动（PLC已连接）")
        else:
            print("[运动控制] ⚠️  已启动（PLC未连接，运动控制不可用）")
    
    def stop(self):
        """停止运动控制"""
        self.running = False
        self.emergency_stop = True
        self.set_target(0, 0)
        print("[运动控制] 已停止")
    
    def _control_loop(self):
        """控制循环 - 实现平滑加减速"""
        loop_count = 0
        plc_warning_shown = False  # 只显示一次PLC未连接警告
        while self.running:
            if self.emergency_stop:
                # 紧急停止 - 立即设置为0
                self.current_forward = 0
                self.current_turn = 0
                if self.plc.connected:
                    self.plc.write(122, 0)
                    self.plc.write(121, 0)
            else:
                # 平滑过渡到目标速度
                # 前进速度
                if abs(self.target_forward - self.current_forward) > 0.1:
                    if self.target_forward > self.current_forward:
                        self.current_forward = min(self.current_forward + self.acceleration, 
                                                  self.target_forward)
                    else:
                        self.current_forward = max(self.current_forward - self.acceleration, 
                                                  self.target_forward)
                else:
                    self.current_forward = self.target_forward
                
                # 转向速度
                if abs(self.target_turn - self.current_turn) > 0.1:
                    if self.target_turn > self.current_turn:
                        self.current_turn = min(self.current_turn + self.acceleration, 
                                               self.target_turn)
                    else:
                        self.current_turn = max(self.current_turn - self.acceleration, 
                                               self.target_turn)
                else:
                    self.current_turn = self.target_turn
                
                # 写入PLC
                if self.plc.connected:
                    plc_warning_shown = False  # 重置警告标志
                    try:
                        loop_count += 1
                        # 只在有运动命令时输出调试信息
                        if (self.current_forward != 0 or self.current_turn != 0) and loop_count % 20 == 0:
                            # 读取控制权限状态
                            control_auth = self.plc.read(120)
                            if control_auth == 0:
                                print(f"[运动控制] ⚠️  控制权限未开启！D120={control_auth} (需要在前端设置为1)")
                            else:
                                print(f"[运动控制] D120={control_auth} | D122(前进)={int(self.current_forward)} | D121(转向)={int(self.current_turn)}")
                        
                        self.plc.write(122, int(self.current_forward))
                        self.plc.write(121, int(self.current_turn))
                    except Exception as e:
                        if loop_count % 20 == 0:
                            print(f"[运动控制] PLC写入异常: {e}")
                else:
                    # PLC未连接，只显示一次警告
                    if not plc_warning_shown:
                        print("[运动控制] ⚠️  PLC未连接（运动控制不可用）")
                        plc_warning_shown = True
            
            time.sleep(0.05)  # 20Hz更新率
    
    def set_target(self, forward, turn):
        """设置目标速度（带安全限制）"""
        # 限制速度范围
        forward = max(-self.max_forward_speed, min(self.max_forward_speed, forward))
        turn = max(-self.max_turn_speed, min(self.max_turn_speed, turn))
        
        self.target_forward = forward
        self.target_turn = turn
        self.emergency_stop = False
    
    def trigger_emergency_stop(self):
        """触发紧急停止"""
        self.emergency_stop = True
        print("[运动控制] 紧急停止!")

class TaskExecutor:
    """任务执行引擎 - 包含路径跟踪算法"""
    def __init__(self, robot_id, plc_controller, motion_controller):
        self.robot_id = robot_id
        self.plc = plc_controller
        self.motion = motion_controller
        self.current_task = None
        self.task_running = False
        self.task_paused = False
        self.current_waypoint_index = 0
        self.path_points = []
        self.current_gps = None
        self.lookahead_distance = 3.0  # 前瞻距离（米）
        self.waypoint_threshold = 2.0  # 到达路径点的阈值（米）
        
    def load_task(self, task_data):
        """加载任务"""
        self.current_task = task_data
        self.path_points = task_data.get('path_points', [])
        self.current_waypoint_index = 0
        self.task_paused = False
        print(f"[任务引擎] 已加载任务: {task_data.get('task_id')}, 路径点数: {len(self.path_points)}")
        
    def start_task(self):
        """开始执行任务"""
        if not self.current_task:
            print("[任务引擎] 错误: 没有加载任务")
            return False
        
        self.task_running = True
        self.task_paused = False
        print(f"[任务引擎] 开始执行任务: {self.current_task.get('task_id')}")
        
        # 启动路径跟踪线程
        threading.Thread(target=self._path_tracking_loop, daemon=True).start()
        return True
    
    def pause_task(self):
        """暂停任务"""
        self.task_paused = True
        self.motion.set_target(0, 0)
        print("[任务引擎] 任务已暂停")
    
    def resume_task(self):
        """恢复任务"""
        self.task_paused = False
        self.motion.emergency_stop = False
        print("[任务引擎] 任务已恢复")
    
    def stop_task(self):
        """停止任务"""
        self.task_running = False
        self.task_paused = False
        self.motion.set_target(0, 0)
        self.current_task = None
        print("[任务引擎] 任务已停止")
    
    def update_gps(self, lat, lon):
        """更新当前GPS位置"""
        self.current_gps = (lat, lon)
    
    def get_progress(self):
        """获取任务进度"""
        if not self.path_points:
            return 0
        return int((self.current_waypoint_index / len(self.path_points)) * 100)
    
    def get_next_waypoint(self):
        """获取下一个路径点"""
        if self.current_waypoint_index < len(self.path_points):
            waypoint = self.path_points[self.current_waypoint_index]
            return waypoint
        return None
    
    def _calculate_distance(self, lat1, lon1, lat2, lon2):
        """计算两点间距离（米）"""
        R = 6371000
        phi1 = math.radians(lat1)
        phi2 = math.radians(lat2)
        delta_phi = math.radians(lat2 - lat1)
        delta_lambda = math.radians(lon2 - lon1)
        a = math.sin(delta_phi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(delta_lambda / 2) ** 2
        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
        return R * c
    
    def _calculate_bearing(self, lat1, lon1, lat2, lon2):
        """计算方位角（度）"""
        phi1 = math.radians(lat1)
        phi2 = math.radians(lat2)
        delta_lambda = math.radians(lon2 - lon1)
        y = math.sin(delta_lambda) * math.cos(phi2)
        x = math.cos(phi1) * math.sin(phi2) - math.sin(phi1) * math.cos(phi2) * math.cos(delta_lambda)
        bearing = math.degrees(math.atan2(y, x))
        return (bearing + 360) % 360
    
    def _path_tracking_loop(self):
        """路径跟踪循环 - Pure Pursuit算法简化版"""
        print("[路径跟踪] 开始")
        
        while self.task_running:
            if self.task_paused or not self.current_gps:
                time.sleep(0.1)
                continue
            
            # 获取当前位置
            current_lat, current_lon = self.current_gps
            
            # 检查是否到达当前路径点
            if self.current_waypoint_index < len(self.path_points):
                target = self.path_points[self.current_waypoint_index]
                target_lat, target_lon = target[0], target[1]
                
                distance_to_target = self._calculate_distance(
                    current_lat, current_lon,
                    target_lat, target_lon
                )
                
                # 到达路径点，移动到下一个
                if distance_to_target < self.waypoint_threshold:
                    self.current_waypoint_index += 1
                    print(f"[路径跟踪] 到达路径点 {self.current_waypoint_index}/{len(self.path_points)}")
                    
                    if self.current_waypoint_index >= len(self.path_points):
                        print("[路径跟踪] 路径完成!")
                        self.stop_task()
                        break
                    continue
                
                # 计算控制命令
                bearing = self._calculate_bearing(
                    current_lat, current_lon,
                    target_lat, target_lon
                )
                
                # 简化的控制逻辑
                # 根据距离调整速度
                if distance_to_target > 10:
                    forward_speed = 20
                elif distance_to_target > 5:
                    forward_speed = 15
                else:
                    forward_speed = 10
                
                # 根据方位角计算转向
                # 假设当前朝向为0度（需要IMU数据才能准确）
                # 这里简化为：左转为负，右转为正
                angle_diff = bearing
                if angle_diff > 180:
                    angle_diff -= 360
                
                turn_speed = max(-12, min(12, angle_diff / 15))
                
                # 发送控制命令
                self.motion.set_target(forward_speed, turn_speed)
                
                # 调试输出
                if self.current_waypoint_index % 5 == 0:
                    print(f"[路径跟踪] 距离:{distance_to_target:.1f}m, 方位:{bearing:.0f}°, "
                          f"前进:{forward_speed}, 转向:{turn_speed:.1f}")
            
            time.sleep(0.2)  # 5Hz控制频率
        
        print("[路径跟踪] 结束")

class RK3588Client:
    def __init__(self, robot_id="GZJY001", server_ip="192.168.10.1"):
        self.robot_id = robot_id
        self.server_ip = server_ip
        self.running = False
        self.cameras = []
        self.processes = []
        # PLC初始化时不指定端口，启动时自动检测
        self.plc = PLCController(port=None)
        self.motion = MotionController(self.plc)
        self.task_executor = TaskExecutor(robot_id, self.plc, self.motion)
        
    def detect_cameras(self):
        """检测摄像头（简化版，参考成功案例）"""
        try:
            result = subprocess.run(['v4l2-ctl', '--list-devices'], capture_output=True, text=True)
            
            print("[摄像头检测] v4l2-ctl扫描结果:")
            for line in result.stdout.split('\n')[:20]:  # 显示前20行帮助调试
                if line.strip():
                    print(f"  {line}")
            
            for pattern, name in [("USB Camera", "前置"), ("LRCP.*F1080P", "后置")]:
                lines = result.stdout.split('\n')
                for i, line in enumerate(lines):
                    if re.search(pattern, line):
                        print(f"[摄像头检测] 匹配到: {line.strip()}")
                        for j in range(i+1, min(i+5, len(lines))):
                            match = re.search(r'/dev/video(\d+)', lines[j])
                            if match:
                                self.cameras.append({
                                    "device": f"/dev/video{match.group(1)}",
                                    "port": 8080 + len(self.cameras),
                                    "name": name
                                })
                                print(f"[摄像头检测] ✓ {name}: /dev/video{match.group(1)}")
                                break
                        break
            
            if len(self.cameras) == 0:
                print("[摄像头检测] ⚠️  未匹配到已知摄像头型号")
            
            return len(self.cameras) > 0
        except Exception as e:
            print(f"[摄像头检测] 错误: {e}")
            return False
    
    def start_camera(self, cam):
        """启动摄像头（简化版，参考成功案例）"""
        cmd = ['gst-launch-1.0', '-q', 'v4l2src', f'device={cam["device"]}',
               '!', 'image/jpeg,width=1280,height=720,framerate=30/1',
               '!', 'multipartmux', 'boundary=frame',
               '!', 'tcpserversink', 'host=0.0.0.0', f'port={cam["port"]}', 'sync=false']
        try:
            p = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
            time.sleep(2)
            if p.poll() is None:
                print(f"[{cam['name']}] ✓ 启动成功 端口:{cam['port']}")
                return p
            else:
                print(f"[{cam['name']}] ✗ 进程退出")
        except Exception as e:
            print(f"[{cam['name']}] ✗ 启动异常: {e}")
        return None
    
    def lidar_loop(self):
        """激光雷达线程"""
        if not serial:
            return
        
        # 检测激光雷达设备
        lidar_device = None
        for device in ['/dev/ttyACM0', '/dev/ttyACM1']:
            if os.path.exists(device):
                lidar_device = device
                break
        
        if not lidar_device:
            print("[雷达] 未检测到激光雷达设备")
            return
        
        try:
            ser = serial.Serial(lidar_device, 230400, timeout=1)
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.connect((self.server_ip, 9001))
            print(f"[雷达] ✓ 已连接: {lidar_device}")
            
            scan_data = [None] * 360
            buffer = bytearray()
            last_send = time.time()
            
            while self.running:
                data = ser.read(1024)
                if data:
                    buffer.extend(data)
                    while len(buffer) >= 47:
                        if buffer[0] == 0x54:
                            packet = buffer[:47]
                            buffer = buffer[47:]
                            try:
                                start_angle = struct.unpack('<H', packet[4:6])[0] / 100.0
                                for i in range(12):
                                    offset = 6 + i * 3
                                    distance = struct.unpack('<H', packet[offset:offset+2])[0]
                                    intensity = packet[offset+2]
                                    if distance > 0:
                                        angle = start_angle + i * 0.75
                                        if angle >= 360:
                                            angle -= 360
                                        idx = int(angle)
                                        if 0 <= idx < 360:
                                            scan_data[idx] = {'angle': round(angle, 2), 'distance': distance, 'intensity': intensity}
                            except:
                                pass
                        else:
                            buffer = buffer[1:]
                
                if time.time() - last_send >= 1.0:
                    points = [p for p in scan_data if p]
                    
                    # 障碍物检测 - 检测前方30度范围内的障碍物
                    obstacle_detected = False
                    min_safe_distance = 500  # 最小安全距离（毫米）
                    obstacle_distance = float('inf')
                    
                    # 检测前方区域（-15度到+15度）
                    for angle in range(-15, 16):
                        idx = angle % 360
                        if scan_data[idx] and scan_data[idx]['distance'] > 0:
                            dist = scan_data[idx]['distance']
                            if dist < min_safe_distance:
                                obstacle_detected = True
                                obstacle_distance = min(obstacle_distance, dist)
                    
                    # 如果检测到障碍物且任务正在运行，触发紧急停止
                    if obstacle_detected:
                        if self.task_executor.task_running and not self.task_executor.task_paused:
                            print(f"[障碍物检测] 前方{obstacle_distance}mm处检测到障碍物！紧急停止")
                            self.motion.trigger_emergency_stop()
                            self.task_executor.pause_task()
                    
                    sock.send((json.dumps({
                        'type': 'lidar_parsed',
                        'robot_id': self.robot_id,
                        'points': points,
                        'point_count': len(points),
                        'obstacle_detected': obstacle_detected,
                        'obstacle_distance': obstacle_distance if obstacle_distance != float('inf') else None
                    }) + '\n').encode())
                    last_send = time.time()
            
            ser.close()
            sock.close()
        except Exception as e:
            print(f"[雷达] 错误: {e}")
    
    def detect_gps_device(self):
        """检测GPS设备（避开ttyUSB0，优先给PLC使用）"""
        # ttyUSB0通常是USB转RS485，留给PLC使用
        # GPS优先使用其他端口
        possible_devices = ['/dev/ttyUSB1', '/dev/ttyUSB2', '/dev/ttyAMA0', '/dev/ttyACM1']
        for device in possible_devices:
            if os.path.exists(device):
                return device
        return None
    
    def gps_loop(self):
        """GPS线程 - 仅支持真实GPS设备"""
        # 检测GPS设备
        gps_device = None
        if serial:
            gps_device = self.detect_gps_device()
        
        if not gps_device:
            print("[GPS] ⚠️  未找到GPS硬件设备，GPS功能不可用")
            return
        
        try:
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.connect((self.server_ip, 9002))
            print("[GPS] ✓ 已连接到服务器")
            
            # 打开GPS设备
            try:
                ser = serial.Serial(gps_device, 9600, timeout=1)
                print(f"[GPS] ✓ 硬件启动: {gps_device}")
            except Exception as e:
                print(f"[GPS] ✗ 无法打开设备 {gps_device}: {e}")
                sock.close()
                return
            
            # GPS数据初始化
            gps_data = {
                'latitude': 0,
                'longitude': 0,
                'satellites': 0,
                'fix_type': 'No Fix',
                'altitude': 0,
                'speed': 0.0,
                'heading': 0.0
            }
            
            send_count = 0
            
            print(f"[GPS] 等待卫星信号...")
            
            while self.running:
                try:
                    if ser.in_waiting > 0:
                        # 真实GPS数据解析
                        line = ser.readline().decode('ascii', errors='ignore').strip()
                        
                        if line.startswith('$GPGGA') or line.startswith('$GNGGA'):
                            parts = line.split(',')
                            if len(parts) >= 15:
                                try:
                                    if parts[2] and parts[3]:
                                        lat = float(parts[2][:2]) + float(parts[2][2:]) / 60.0
                                        gps_data['latitude'] = -lat if parts[3] == 'S' else lat
                                    if parts[4] and parts[5]:
                                        lon = float(parts[4][:3]) + float(parts[4][3:]) / 60.0
                                        gps_data['longitude'] = -lon if parts[5] == 'W' else lon
                                    if parts[7]:
                                        gps_data['satellites'] = int(parts[7])
                                    if parts[6]:
                                        gps_data['fix_type'] = {'0': 'No Fix', '1': 'GPS Fix', '2': 'DGPS Fix'}.get(parts[6], 'Unknown')
                                    if parts[9]:
                                        gps_data['altitude'] = float(parts[9])
                                except Exception as e:
                                    print(f"[GPS] 解析错误: {e}")
                        
                        elif line.startswith('$GPRMC') or line.startswith('$GNRMC'):
                            parts = line.split(',')
                            if len(parts) >= 8:
                                try:
                                    if parts[7]:  # 速度(节)
                                        gps_data['speed'] = float(parts[7]) * 1.852  # 转换为km/h
                                    if parts[8]:  # 航向
                                        gps_data['heading'] = float(parts[8])
                                except Exception as e:
                                    pass
                    
                    # 只有有效数据才发送
                    if gps_data['latitude'] != 0 or gps_data['longitude'] != 0:
                        # 发送GPS数据
                        sock.send((json.dumps({
                            'type': 'gps_data',
                            'robot_id': self.robot_id,
                            'data': gps_data
                        }) + '\n').encode())
                        
                        # 更新任务执行器的GPS位置
                        self.task_executor.update_gps(gps_data['latitude'], gps_data['longitude'])
                        
                        send_count += 1
                        if send_count % 30 == 1:
                            print(f"[GPS] #{send_count}: {gps_data['latitude']:.6f}, {gps_data['longitude']:.6f} | "
                                  f"卫星:{gps_data['satellites']} 速度:{gps_data['speed']:.1f}km/h")
                
                except Exception as e:
                    print(f"[GPS] 数据处理错误: {e}")
                
                time.sleep(0.5)
            
            if ser:
                ser.close()
            sock.close()
            print("[GPS] 线程已停止")
            
        except Exception as e:
            print(f"[GPS] 致命错误: {e}")
    
    def plc_loop(self):
        """PLC线程"""
        if not self.plc.connect():
            print("[PLC] 连接失败")
            return
        
        try:
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.connect((self.server_ip, 9003))
            print("[PLC] 数据通道连接")
            
            while self.running:
                sensors = self.plc.read_all()
                sock.send((json.dumps({
                    'type': 'sensor_data',
                    'robot_id': self.robot_id,
                    'sensors': sensors
                }) + '\n').encode())
                time.sleep(2)
            
            sock.close()
        except Exception as e:
            print(f"[PLC] 错误: {e}")
    
    def control_loop(self):
        """命令接收线程 + 心跳发送"""
        while self.running:
            try:
                sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                sock.connect((self.server_ip, 9000))
                print("[命令] 连接成功")
                
                # 立即发送一次video消息，让Windows保存socket
                sock.send((json.dumps({
                    'type': 'video',
                    'robot_id': self.robot_id,
                    'data': {'connected': True, 'cameras': len(self.processes)}
                }) + '\n').encode())
                print(f"[命令] 已发送robot_id: {self.robot_id}")
                
                buffer = ""
                last_heartbeat = time.time()
                
                while self.running:
                    # 发送心跳（每10秒）
                    if time.time() - last_heartbeat >= 10:
                        try:
                            sock.send((json.dumps({
                                'type': 'heartbeat',
                                'robot_id': self.robot_id,
                                'timestamp': time.time(),
                                'status': {
                                    'plc_connected': self.plc.connected,
                                    'cameras': len(self.processes),
                                    'task_running': self.task_executor.task_running if hasattr(self, 'task_executor') else False
                                }
                            }) + '\n').encode())
                            print(f"[心跳] 已发送")
                        except Exception as e:
                            print(f"[心跳] 发送失败: {e}")
                            break
                        last_heartbeat = time.time()
                    
                    # 接收命令
                    sock.settimeout(0.1)
                    try:
                        data = sock.recv(1024)
                        if data:
                            print(f"[DEBUG] 收到{len(data)}字节")
                            buffer += data.decode('utf-8')
                            while '\n' in buffer:
                                line, buffer = buffer.split('\n', 1)
                                if line.strip():
                                    print(f"[DEBUG] 解析行: {line[:50]}")
                                    try:
                                        cmd = json.loads(line)
                                        print(f"[DEBUG] JSON成功: {cmd}")
                                        self.process_cmd(cmd)
                                    except Exception as e:
                                        print(f"[命令] 处理异常: {e}")
                                        import traceback
                                        traceback.print_exc()
                    except socket.timeout:
                        pass
                    except Exception as e:
                        print(f"[命令] 接收异常: {e}")
                    
                    time.sleep(0.05)
                
                sock.close()
            except Exception as e:
                print(f"[命令] 连接失败: {e}")
                time.sleep(5)
    
    def process_cmd(self, cmd):
        """处理命令"""
        try:
            cmd_type = cmd.get('type')
            print(f"[收到] 类型={cmd_type}")
            
            if cmd_type == 'movement':
                forward = cmd.get('forward', 0)
                turn = cmd.get('turn', 0)
                print(f"{'='*60}")
                print(f"[WASD运动控制] 收到命令")
                print(f"  前进速度: {forward}")
                print(f"  转向速度: {turn}")
                print(f"  PLC连接状态: {self.plc.connected}")
                if self.plc.connected:
                    control_auth = self.plc.read(120)
                    print(f"  控制权限D120: {control_auth} {'✓ 远程控制' if control_auth == 1 else '✗ 本地遥控（需设置为1）'}")
                print(f"{'='*60}")
                
                # 使用运动控制器（带平滑和安全限制）
                self.motion.set_target(forward, turn)
            
            elif cmd_type == 'control':
                ctype = cmd.get('control_type')
                value = cmd.get('value', 0)
                register = cmd.get('register', 0)
                
                # PLC控制类型映射（用于日志显示）
                control_names = {
                    'control_authority': '控制权限',
                    'speed_percent': '行进速度百分比',
                    'generator_start': '发电机控制',
                    'relay_220v': '220V继电器',
                    'water_spray': '喷水控制',
                    'cooling_control': '制冷控制',
                    'blade_control': '割叶机/刀具控制'
                }
                
                control_name = control_names.get(ctype, ctype)
                print(f"{'='*60}")
                print(f"[PLC控制] {control_name}")
                print(f"  寄存器: D{register}")
                print(f"  设置值: {value}")
                
                if self.plc.connected:
                    def write_control():
                        try:
                            # 根据control_type写入对应的寄存器
                            if ctype == 'control_authority':
                                self.plc.write(120, value)
                                mode_name = '远程控制' if value == 1 else '本地遥控'
                                print(f"  ✓ 控制权限已设置为: {mode_name}")
                            elif ctype == 'speed_percent':
                                self.plc.write(127, value)
                                print(f"  ✓ 行进速度已设置为: {value}%")
                            elif ctype == 'generator_start':
                                self.plc.write(123, value)
                                status = {0: '关闭', 1: '启动', 2: '停机'}.get(value, '未知')
                                print(f"  ✓ 发电机控制: {status}")
                            elif ctype == 'relay_220v':
                                self.plc.write(124, value)
                                status = '打开' if value == 1 else '关闭'
                                print(f"  ✓ 220V继电器: {status}")
                            elif ctype == 'water_spray':
                                self.plc.write(125, value)
                                status = '打开' if value == 1 else '关闭'
                                print(f"  ✓ 喷水控制: {status}")
                            elif ctype == 'cooling_control':
                                self.plc.write(126, value)
                                status = {0: '关闭', 1: '手动启动', 2: '自动模式'}.get(value, '未知')
                                print(f"  ✓ 制冷控制: {status}")
                            elif ctype == 'blade_control':
                                self.plc.write(128, value)
                                status = '运行中' if value == 1 else '关闭'
                                print(f"  ✓ 割叶机/刀具: {status}")
                            print(f"{'='*60}")
                        except Exception as e:
                            print(f"  ✗ PLC写入失败: {e}")
                            print(f"{'='*60}")
                    threading.Thread(target=write_control, daemon=True).start()
                else:
                    print(f"  ✗ PLC未连接，无法执行控制")
                    print(f"{'='*60}")
                    
            elif cmd_type == 'continuous_forward':
                duration = cmd.get('duration', 5000) / 1000.0
                print(f"[测试] 前进{duration}秒")
                
                if self.plc.connected:
                    def test_forward():
                        try:
                            self.plc.write(122, 15)
                            time.sleep(duration)
                            self.plc.write(122, 0)
                            print("[测试] 完成")
                        except:
                            pass
                    threading.Thread(target=test_forward, daemon=True).start()
            
            elif cmd_type == 'path_task':
                # 任务分配命令
                task_id = cmd.get('task_id')
                path_name = cmd.get('path_name')
                print(f"[任务引擎] 收到任务分配: {task_id} - {path_name}")
                self.task_executor.load_task(cmd)
                
            elif cmd_type == 'task_start':
                # 启动任务
                print("[任务引擎] 收到启动命令")
                self.task_executor.start_task()
                
            elif cmd_type == 'task_pause':
                # 暂停任务
                print("[任务引擎] 收到暂停命令")
                self.task_executor.pause_task()
                
            elif cmd_type == 'task_resume':
                # 恢复任务
                print("[任务引擎] 收到恢复命令")
                self.task_executor.resume_task()
                
            elif cmd_type == 'task_stop':
                # 停止任务
                print("[任务引擎] 收到停止命令")
                self.task_executor.stop_task()
                    
        except Exception as e:
            print(f"[命令] 处理错误: {e}")
    
    def cleanup(self):
        """清理函数 - 程序退出时调用"""
        print("\n" + "=" * 60)
        print("  程序退出清理")
        print("=" * 60)
        
        # 停止所有线程
        self.running = False
        
        # 停止运动控制
        if hasattr(self, 'motion'):
            self.motion.stop()
            print("[清理] 运动控制已停止")
        
        # 释放PLC控制权限（D120设置为0）
        if self.plc.connected:
            try:
                print("[清理] 正在释放PLC控制权限...")
                self.plc.write(120, 0)  # D120 = 0 (本地遥控)
                print("[清理] ✓ 控制权限已释放 (D120 = 0)")
                
                # 关闭所有PLC输出
                print("[清理] 正在关闭PLC输出...")
                self.plc.write(122, 0)  # 停止行进
                self.plc.write(121, 0)  # 停止转向
                self.plc.write(124, 0)  # 关闭220V继电器
                self.plc.write(125, 0)  # 关闭喷水
                self.plc.write(128, 0)  # 关闭割叶机
                print("[清理] ✓ PLC输出已全部关闭")
            except Exception as e:
                print(f"[清理] ⚠️  PLC清理失败: {e}")
        
        # 停止摄像头进程
        for p in self.processes:
            if p:
                try:
                    p.terminate()
                except:
                    pass
        print("[清理] 摄像头进程已停止")
        
        print("=" * 60)
        print("  清理完成，再见！")
        print("=" * 60)
    
    def start(self):
        """启动客户端"""
        print("=" * 60)
        print(f"  机器人: {self.robot_id}")
        print(f"  服务器: {self.server_ip}")
        print("=" * 60)
        
        # 设备状态汇总
        device_status = []
        
        # 检测摄像头
        if self.detect_cameras():
            device_status.append(f"✓ 摄像头: {len(self.cameras)}个")
        else:
            device_status.append("✗ 摄像头: 未检测到")
        
        # 清理旧进程
        subprocess.run(['pkill', '-f', 'gst-launch'], capture_output=True)
        time.sleep(1)
        
        # 启动摄像头
        for cam in self.cameras:
            subprocess.run(['chmod', '666', cam['device']], capture_output=True)
            p = self.start_camera(cam)
            if p:
                self.processes.append(p)
        
        self.running = True
        
        # 启动运动控制器
        self.motion.start()
        
        # 启动所有线程
        threading.Thread(target=self.control_loop, daemon=True).start()
        threading.Thread(target=self.lidar_loop, daemon=True).start()
        threading.Thread(target=self.gps_loop, daemon=True).start()
        threading.Thread(target=self.plc_loop, daemon=True).start()
        
        # 等待一下让所有线程启动并检测设备
        time.sleep(1)
        
        # 显示设备状态汇总
        print("\n" + "=" * 60)
        print("  设备状态汇总")
        print("-" * 60)
        
        if self.plc.connected:
            device_status.append(f"✓ PLC: {self.plc.port}")
        else:
            device_status.append("✗ PLC: 未连接（运动控制不可用）")
        
        # 这里不检查雷达和GPS，因为它们在线程中异步启动
        device_status.append("  激光雷达: 检测中...")
        device_status.append("  GPS: 检测中...")
        
        for status in device_status:
            print(f"  {status}")
        
        print("=" * 60)
        print("  运行中... Ctrl+C 停止")
        print("=" * 60)
        
        try:
            while self.running:
                time.sleep(1)
        except KeyboardInterrupt:
            print("\n[停止] 收到Ctrl+C信号")
        finally:
            self.cleanup()

if __name__ == "__main__":
    import sys
    robot_id = sys.argv[1] if len(sys.argv) > 1 else "GZJY001"
    server_ip = sys.argv[2] if len(sys.argv) > 2 else "192.168.10.1"
    
    client = RK3588Client(robot_id, server_ip)
    client.start()

